#include "Stepper_driver.h"
#include "log.h"
#include "ITM.h"
#include "string.h"
#define LOGTAG "Stepper"
#define StepperLog(fmt, ...) LOG_DBG(LOGTAG,fmt,##__VA_ARGS__)
// tim channel -> dma channel id
static uint8_t dmaIndex[4] = {TIM_DMA_ID_CC1, TIM_DMA_ID_CC2, TIM_DMA_ID_CC3, TIM_DMA_ID_CC4};
static uint8_t timitIndex[4] = {TIM_IT_CC1, TIM_IT_CC2, TIM_IT_CC3, TIM_IT_CC4};
// static uint16_t hsd->stepMap[STEPPER_DATAMAP_SIZE] = {0};
#define PinDEBUG1(x) do{(x) > 0 ? (DEBUG1_GPIO_Port->BSRR |= DEBUG1_Pin) : (DEBUG1_GPIO_Port->BRR |= DEBUG1_Pin);}while(0)
#define PinDEBUG2(x) do{(x) > 0 ? (DEBUG2_GPIO_Port->BSRR |= DEBUG2_Pin) : (DEBUG2_GPIO_Port->BRR |= DEBUG2_Pin);}while(0)
void StepperDriverInit(stepperDriver_t *hsd)
{
    hsd->curDir = 0;
    hsd->curStep = 0;
    hsd->planStep = 0;
    hsd->endPulse = 0;
    hsd->dirCtrl.gpio = NULL;
    hsd->enCtrl.gpio = NULL;
    hsd->stepCtrl.htim = NULL;
}
void StepperDriverAttachPulsePin(stepperDriver_t *hsd,
                                 GPIO_TypeDef *gpio,
                                 uint16_t pinBit,
                                 TIM_HandleTypeDef *htim,
                                 uint8_t channel)
{
    hsd->stepCtrl.gpio = gpio;
    hsd->stepCtrl.pinBit = pinBit;
    hsd->stepCtrl.htim = htim;
    hsd->stepCtrl.channel = channel;
}

void StepperDriverAttachDirPin(stepperDriver_t *hsd,
                               GPIO_TypeDef *gpio,
                               uint16_t pinBit,
                               uint16_t defaultVoltage)
{
    hsd->dirCtrl.gpio = gpio;
    hsd->dirCtrl.pinBit = pinBit;
    hsd->dirCtrl.defaultVoltage = defaultVoltage;
}

void StepperDriverAttachEnPin(stepperDriver_t *hsd,
                              GPIO_TypeDef *gpio,
                              uint16_t pinBit,
                              uint16_t defaultVoltage)
{
    hsd->enCtrl.gpio = gpio;
    hsd->enCtrl.pinBit = pinBit;
    hsd->enCtrl.defaultVoltage = defaultVoltage;
}

/**
 * @brief 设置使能引脚状态
 * @param hsd 
 * @param status 
 */
void StepperDriverSetEnableStatus(stepperDriver_t *hsd, uint8_t status)
{
    StepperAssert(hsd);
    uint32_t pinState;
    if (status == 0) //close
    {
        StepperDriverStopMove(hsd);
        pinState = hsd->enCtrl.defaultVoltage;
    }
    else
    {
        pinState = !hsd->enCtrl.defaultVoltage;
    }
    HAL_GPIO_WritePin(hsd->enCtrl.gpio, hsd->enCtrl.pinBit,
                      (GPIO_PinState)pinState);
}
uint32_t StepperDriverGetEnableStatus(stepperDriver_t *hsd)
{
    return (HAL_GPIO_ReadPin(hsd->enCtrl.gpio, hsd->enCtrl.pinBit)
            == hsd->enCtrl.defaultVoltage ? 0 : 1);
}

void StepperDriverSetDir(stepperDriver_t *hsd, int dir)
{
    HAL_GPIO_WritePin(hsd->dirCtrl.gpio,
                      hsd->dirCtrl.pinBit,
                      (GPIO_PinState)((dir>0) ? !hsd->dirCtrl.defaultVoltage : hsd->dirCtrl.defaultVoltage));
}
int StepperDriverGetDir(stepperDriver_t *hsd)
{
    return (HAL_GPIO_ReadPin(hsd->dirCtrl.gpio, hsd->dirCtrl.pinBit)
            == hsd->dirCtrl.defaultVoltage ? 1 : -1);
}

StepperDriverState_t StepperDriverMoveSteps(stepperDriver_t *hsd, long steps)
{
    StepperAssert(hsd);
    if (StepperDriverGetEnableStatus(hsd) == 0) return STEPPER_DRIVER_ERROR;
    if (hsd->curStep > 0) return STEPPER_DRIVER_BUSY;
    /* 方向处理 */
    if (steps > 0)
    {
        StepperDriverSetDir(hsd, 1);
        hsd->curStep = steps;
        hsd->curDir = 1;
    }
    else if (steps < 0)
    {
        StepperDriverSetDir(hsd, -1);
        hsd->curStep = -steps;
        hsd->curDir = -1;
    }
    else return STEPPER_DRIVER_ERROR;
    /* 脉冲数计算 */
    StepperLog("channel:%d, startStep:%d\n", hsd->stepCtrl.channel, steps);
    if (hsd->curStep*2 > STEPPER_DATAMAP_SIZE) /* 超过DMA缓存的部分存起来,分段多次传输 */
    {
        hsd->planStep = hsd->curStep - STEPPER_DATAMAP_SIZE/2;
        hsd->curStep = STEPPER_DATAMAP_SIZE/2;
    }
    /* 定时器设置 */
    __HAL_TIM_SET_COMPARE(hsd->stepCtrl.htim, hsd->stepCtrl.channel, hsd->stepMap[0]);
    __HAL_TIM_SET_COUNTER(hsd->stepCtrl.htim, 0);
    PinDEBUG2(1); //debug2引脚高电平代表开始DMA和定时器
    hsd->stepCtrl.htim->Instance->EGR = TIM_EGR_UG;
    HAL_TIM_OC_Start_DMA(hsd->stepCtrl.htim,
                          hsd->stepCtrl.channel,
                          (uint32_t*)&hsd->stepMap[1],
                          hsd->curStep*2-1);
    LOG_DBG(LOGTAG, "Pulse start :Cnt%d, CMP:%d\n",hsd->stepCtrl.htim->Instance->CNT, hsd->stepCtrl.htim->Instance->CCR3);
    return STEPPER_DRIVER_OK;
}
#if 1
void StepperDriverStopMove(stepperDriver_t *hsd)
{
    uint16_t waitStep;
    TIM_HandleTypeDef *htim = hsd->stepCtrl.htim;
    uint16_t ch = hsd->stepCtrl.channel;
    int curstep = 0;
    if (hsd->curStep == 0) return;
    hsd->planStep = 0;
    if (__HAL_TIM_GET_ITSTATUS(htim, timitIndex[hsd->stepCtrl.channel>>2]) == SET) return;
    curstep = hsd->curStep;
    hsd->curStep = 0;

    while ((hsd->stepCtrl.gpio->IDR & hsd->stepCtrl.pinBit) > 0); //等待低电平
    __HAL_TIM_SET_COMPARE(htim, ch, (uint16_t)(htim->Instance->CNT - 1)); //防止再次翻转
    HAL_TIM_OC_Stop_DMA(hsd->stepCtrl.htim, hsd->stepCtrl.channel);
    waitStep = (__HAL_DMA_GET_COUNTER(htim->hdma[dmaIndex[ch>>2]])+1)/2;
    hsd->stepCnt += (curstep - waitStep)*hsd->curDir;
    hsd->curDir = 0;
    curstep = 0;
    PinDEBUG2(0);
}
#else
void StepperDriverStopMove(stepperDriver_t *hsd)
{
    uint16_t waitStep;
    TIM_HandleTypeDef *htim = hsd->stepCtrl.htim;
    uint16_t ch = hsd->stepCtrl.channel;
    int curstep;

    if (hsd->curStep == 0) return;
    curstep = hsd->curStep;
    hsd->curStep = 0; //避免进入中断
    __HAL_TIM_DISABLE_IT(htim, timitIndex[ch>>2]);
	
	// __HAL_TIM_DISABLE_DMA(htim, TIM_DMA_CC1<<(ch>>2));
    // HAL_DMA_Abort_IT(htim->hdma[dmaIndex[ch>>2]]);

    // waitStep = __HAL_DMA_GET_COUNTER(htim->hdma[dmaIndex[ch>>2]]);
    // if ((waitStep&0x01) == 0)
    // {
    //     __HAL_TIM_SET_COMPARE(htim, ch, htim->Instance->CNT+1);
    //     while(__HAL_TIM_GET_COMPARE(htim,ch) != htim->Instance->CNT);
    //     waitStep = waitStep/2;
    // }
    __HAL_TIM_DISABLE_DMA(&htim2, TIM_DMA_CC1);
    HAL_DMA_Abort_IT(htim2.hdma[TIM_DMA_ID_CC1]);
    TIM2->CCR1 = (uint16_t)(TIM2->CNT + 100); //避免在此触发比较
    waitStep = __HAL_DMA_GET_COUNTER(htim2.hdma[TIM_DMA_ID_CC1]);

    if ((GPIOA->IDR & GPIO_PIN_0) == 0 && (waitStep&0x01))
    {
        HAL_TIM_OC_Stop_DMA(&htim2, TIM_CHANNEL_1);
        waitStep = (waitStep + 1)/2;
    }
    else
    {
        TIM2->CCR1 = (uint16_t)(TIM2->CNT+1);
        // while(TIM2->CCR1 > TIM2->CNT);
        // __NOP();__NOP();__NOP();__NOP();
        // __NOP();__NOP();__NOP();__NOP();
        while ((GPIOA->IDR & GPIO_PIN_0) > 0);
        HAL_TIM_OC_Stop_DMA(&htim2, TIM_CHANNEL_1);
        waitStep = waitStep/2;
    }
    
    PinDEBUG2(0);

    hsd->stepCnt += (curstep - waitStep)*hsd->curDir;
    hsd->planStep = 0;
    hsd->curDir = 0;
    curstep = 0;
    StepperLog("stepper stop pos:%lld,wait:%d\n", hsd->stepCnt,waitStep);
    
}
#endif

StepperDriverState_t StepperDriverMovePos(stepperDriver_t *hsd, long long position)
{
    return StepperDriverMoveSteps(hsd, position - hsd->stepCnt);
}

void StepperDriverSetFrequnce(stepperDriver_t *hsd, uint32_t freq)
{
    uint32_t baseFreq = STEPPER_TIM_CLOCK/(StepperGetPrescaler(hsd)+1)/2; //two data one pulse

    if (freq > baseFreq)
    {
        freq = baseFreq;
    }
    else if (freq < baseFreq/StepperGetPeriod(hsd))
    {
        freq = (uint32_t)((float)baseFreq/StepperGetPeriod(hsd)+0.5);
    }

    hsd->stepMap[0] = (uint16_t)(baseFreq/freq);
    for (size_t i = 1; i < STEPPER_DATAMAP_SIZE; i++)
    {
        hsd->stepMap[i] = hsd->stepMap[i-1] + hsd->stepMap[0];
    }
    
    StepperLog("FreqacSet:%d\n",freq);
}

uint32_t StepperDriverGetFrequnce(stepperDriver_t *hsd)
{
    return STEPPER_TIM_CLOCK/((StepperGetPrescaler(hsd)+1)*hsd->stepMap[0])*2;
}

StepperDriverState_t StepperDriverGetState(stepperDriver_t *hsd)
{
    return hsd->curStep > 0 ? STEPPER_DRIVER_BUSY : STEPPER_DRIVER_OK;
}

long long StepperDriverGetStep(stepperDriver_t *hsd)
{
    uint32_t waitStep = (__HAL_DMA_GET_COUNTER(hsd->stepCtrl.htim->hdma[dmaIndex[hsd->stepCtrl.channel>>2]])+1)/2;
    // StepperLog("wait:%d,curCnt:%d\n", waitStep, (hsd->curStep - waitStep) * hsd->curDir);
    return hsd->stepCnt + (long long)(hsd->curStep - waitStep) * (long long)hsd->curDir;
}


void StepperDriverPulseFinishedCallback(stepperDriver_t *hsd, TIM_HandleTypeDef *htim)
{
    if (htim != hsd->stepCtrl.htim || hsd->curStep == 0)
        return;
    PinDEBUG1(1);//Debug1引脚设置高电平,表示进入中断

    if (__HAL_TIM_GET_ITSTATUS(htim, timitIndex[hsd->stepCtrl.channel>>2]) == RESET)
    {
        //DMA传输完成，第一次中断进入到这里
        __HAL_TIM_ENABLE_IT(htim, timitIndex[hsd->stepCtrl.channel>>2]);
        hsd->endPulse = 1; //开启了捕获/比较中断,定时器中断马上调用了一次回调,使用标志位跳过
        PinDEBUG1(0); //Debug1引脚设置低电平,表示退出中断
        return; 
    }
    if (hsd->endPulse == 1)
    {
        //跳过因开启中断而导致立即进入的中断
        hsd->endPulse = 0;
        PinDEBUG1(0); //Debug1引脚设置低电平,表示退出中断
        return; 
    }
    //最后一个输出比较结束，第三次进入回调
    PinDEBUG2(0); //debug2引脚设置低电平,高电平的持续时间就是本次定时器脉冲的总时长
    __HAL_TIM_DISABLE_IT(htim, timitIndex[hsd->stepCtrl.channel>>2]);
    HAL_TIM_OC_Stop_DMA(hsd->stepCtrl.htim, hsd->stepCtrl.channel);
    
    hsd->stepCnt += hsd->curStep*hsd->curDir; // 计算累计步数
    if (hsd->planStep > 0) //还有计划的步数没走完
    {
        if (hsd->planStep*2 > STEPPER_DATAMAP_SIZE)
        {
            hsd->planStep -= STEPPER_DATAMAP_SIZE/2;
            hsd->curStep = STEPPER_DATAMAP_SIZE/2;
        }
        else
        {
            hsd->curStep = hsd->planStep;
            hsd->planStep = 0;
        }

        __HAL_TIM_SET_COMPARE(hsd->stepCtrl.htim, hsd->stepCtrl.channel, hsd->stepMap[0]);
        __HAL_TIM_SET_COUNTER(hsd->stepCtrl.htim, 0);
        PinDEBUG2(1); //debug2引脚高电平代表开始DMA和定时器
        hsd->stepCtrl.htim->Instance->EGR = TIM_EGR_UG;
        HAL_TIM_OC_Start_DMA(hsd->stepCtrl.htim,
                            hsd->stepCtrl.channel,
                            (uint32_t*)&hsd->stepMap[1],
                            hsd->curStep*2-1);
    }
    else
    {
        StepperLog("channel:%d stepCnt:%lld\n",hsd->stepCtrl.channel, hsd->stepCnt);
        hsd->planStep = 0;
        hsd->curStep = 0;
        hsd->curDir = 0;
        hsd->stepCtrl.htim->Instance->SR = 0;
        
    }
    PinDEBUG1(0); //Debug1引脚设置低电平,表示退出中断
}

__weak void StepperAssert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
	while(1);
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
